#include "stlObject.h"
#include <cassert>

#include <sstream>

namespace TorLib {

StlObject::StlObject()
{
	isLoaded = false;
}

StlObject::StlObject( const std::string& filename )
{
	_identity = filename;
	if( _readStlBinary(filename) )
	{
		isLoaded = true;
	}else
	{
		isLoaded = false;
	}
}

StlObject::~StlObject()
{
	_vertices.clear();
	_normals.clear();
}

unsigned long StlObject::_getUli() {

	unsigned char byte1,byte2,byte3,byte4;
	unsigned long int number;
	byte1=(unsigned char)fgetc( _stl_file );
	byte2=(unsigned char)fgetc( _stl_file );
	byte3=(unsigned char)fgetc( _stl_file );
	byte4=(unsigned char)fgetc( _stl_file );
	number = (unsigned long int)byte1 + ((unsigned long int)byte2<<8)
		+ ((unsigned long int)byte3<<16) + ((unsigned long int)byte4<<24);
	return( number );
}


float StlObject::_getFloat() {
	float number;
	fread(&number, 4, 1, _stl_file); // 800k vert file loadtime from 12 secs to 8secs

	//float number;
	//unsigned char stream[5];
	//// used to be 3,2,1,0
	//stream[0]=(unsigned char)fgetc( _stl_file ); //fgetc is slow
	//stream[1]=(unsigned char)fgetc( _stl_file );
	//stream[2]=(unsigned char)fgetc( _stl_file );
	//stream[3]=(unsigned char)fgetc( _stl_file );

	//stream[4] = '\0';

	//memcpy( &number, &stream, 4 );

	return( number );
}


unsigned int StlObject::_getUint() {

	unsigned char byte1,byte2;
	unsigned int number;
	byte1=(unsigned char)fgetc( _stl_file );
	byte2=(unsigned char)fgetc( _stl_file );
	number = (unsigned int)byte1 + ((unsigned int)byte2<<8);
	return( number );
}

bool StlObject::_readStlBinary(const std::string& filename) {

	if( ( _stl_file = fopen( filename.c_str(), "rb" ) ) == NULL )
		return(false);

	//fopen_s( FILE, filename, mode)
	// explained on http://msdn.microsoft.com/en-us/library/z5hh6ee9%28VS.80%29.aspx
	//errno_t err;
	//if( ( err = fopen_s( &_stl_file, filename.c_str(), "rb" ) ) == NULL )
	//	return(-1);

	unsigned long int t;

	// Skipping comment line
	for( t=0; t<80 ; t++ )
		fgetc( _stl_file );

	// Get number for triangles
	unsigned long int n_triangles = _getUli();

	_normals.resize( n_triangles );
	_vertices.resize( (n_triangles*3) ); // 3 pts

	// Read all the triangles
	size_t normCount = 0;
	size_t vertCount = 0;
	float xyzBuf[4*3];
	for( t=0; t<n_triangles; t++ ) {
		// 4x speedup. reduced 800k triangle loadtime from 12secs to 3secs
		// could be sped up more. but this seems good enough. and code isnt too unreadable
		// for more on binary io in c++ see
		// http://courses.cs.vt.edu/~cs2604/fall02/binio.html#read
		fread( &xyzBuf[0], 4, 12, _stl_file );
		_normals[normCount++] = XYZ(xyzBuf[0],xyzBuf[1],xyzBuf[2] );
		_vertices[vertCount++] = XYZ(xyzBuf[3],xyzBuf[4],xyzBuf[5] );
		_vertices[vertCount++] = XYZ(xyzBuf[6],xyzBuf[7],xyzBuf[8] );
		_vertices[vertCount++] = XYZ(xyzBuf[9],xyzBuf[10],xyzBuf[11] );
		char dummy[2];
		fread( &dummy[0], 1, 2, _stl_file); // ignore uint

		// slow!
		//_normals.push_back(GMlib::Vector3D<float>(xyzBuf[0],xyzBuf[1],xyzBuf[2]) );
		//_vertices.push_back(GMlib::Vector3D<float>(xyzBuf[3],xyzBuf[4],xyzBuf[5]) );
		//_vertices.push_back(GMlib::Vector3D<float>(xyzBuf[6],xyzBuf[7],xyzBuf[8]) );
		//_vertices.push_back(GMlib::Vector3D<float>(xyzBuf[9],xyzBuf[10],xyzBuf[11]) );
		

		// or the slow way:

		// and put in array
		//x=_getFloat(); y=_getFloat(); z=_getFloat();
		//_normals.push_back( GMlib::Vector3D<float>(x,y,z) );

		//x=_getFloat(); y=_getFloat(); z=_getFloat();
		//_vertices.push_back( GMlib::Vector3D<float>(x,y,z) );

		//x=_getFloat(); y=_getFloat(); z=_getFloat();
		//_vertices.push_back( GMlib::Vector3D<float>(x,y,z) );

		//x=_getFloat(); y=_getFloat(); z=_getFloat();
		//_vertices.push_back( GMlib::Vector3D<float>(x,y,z) );

		// Get number of attribute bytes (is always zero and can be ignored)
		//_getUint();
	}

	fclose( _stl_file );
	return true;
}

void StlObject::save( std::ofstream& stream, bool binary ) {


	// Binary
	if( binary ) {

		std::stringstream header;
		header << "GMlib STL: " << this->getIdentity();

		char hbuff[80]; for( int i = 0; i < 80; i++ ) hbuff[i] = ' ';
		memcpy( hbuff, header.str().c_str(), header.str().length() );
		stream.write( hbuff, 80 );


		unsigned int facets = _normals.size();
		stream.write( (char*)&facets, sizeof( unsigned int ) );

		for( size_t i = 0; i < _normals.size(); i++ ) {

			// Normal
			stream.write( (char*)&_normals[i], 3 * sizeof( unsigned int ) );

			// Vertices
			stream.write( (char*)&_vertices[i*3], 9 * sizeof( unsigned int ) );

			// Attribute
			unsigned short attrib = 0;
			stream.write( (char*)&attrib, sizeof( unsigned short ) );
		}
	}
	else {

		std::stringstream content;
		content << "solid " << this->getIdentity() << std::endl;

		for( size_t i = 0; i < _normals.size(); i++ ) {

			const XYZ &v0 = _vertices[i*3];
			const XYZ &v1 = _vertices[i*3+1];
			const XYZ &v2 = _vertices[i*3+2];

			// Normal
			const XYZ &n = _normals[i];

			content << "  facet normal " << n.x << " " << n.y << " " << n.z << std::endl;

			content << "    outer loop" << std::endl;

			// Vertices
			float scaler = 1e7;
			content << int(v0.x*scaler) << " " << int(v0.y*scaler) << " " << int(v0.z*scaler) << ";"  << std::endl;
			content << int(v1.x*scaler) << " " << int(v1.y*scaler) << " " << int(v1.z*scaler) << ";"  << std::endl;
			content  << int(v2.x*scaler) << " " << int(v2.y*scaler) << " " << int(v2.z*scaler) << ";" << std::endl;

			content << "    endloop" << std::endl;

			content << "  endfacet" << std::endl;
		}

		content << "endsolid " << this->getIdentity() << std::endl;

		stream.write( content.str().c_str(), content.str().length() * sizeof( char ) );
	}
}

const std::vector<XYZ>& StlObject::getNormals()
{
	return _normals;
}

const std::vector<XYZ>& StlObject::getVertices()
{
	return _vertices;
}

std::string StlObject::getIdentity() const
{
	return _identity;
}


void StlObject::setScale( float newScale )
{
	for(unsigned int i=0; i<_vertices.size(); i++ )
	{
		_vertices[i] = XYZ( _vertices[i].x * newScale, _vertices[i].y * newScale , _vertices[i].z * newScale );
	}
}

} // end namespace TorLib